package BeamAndBall;

import SimEnvironment.AnalogSink;
import SimEnvironment.AnalogSource;

// BeamAndBallRegul class to be written by you
public class BeamAndBallRegul extends Thread {
	
	private ReferenceGenerator referenceGenerator;
	private PI PIController;
	private PID PIDController;

	//output of process beam and angle
	private AnalogSource analogInPosition; // to input of PID controller
	private AnalogSource analogInAngle; // to input of PI controller
	
	private AnalogSink analogOut;
	private AnalogSink analogRef;

	private double uMin = -10.0;
	private double uMax = 10.0;

	// Constructor
	public BeamAndBallRegul(ReferenceGenerator ref, BeamAndBall beamandball,
			int pri) {
		referenceGenerator = ref;
		PIController = new PI("PI");
		PIDController = new PID("PID");

		//process output
		analogInPosition = beamandball.getSource(0);
		analogInAngle = beamandball.getSource(1);
		
		//process input and reference
		analogOut = beamandball.getSink(0);
		analogRef = beamandball.getSink(1);
		setPriority(pri);
	}

	public void run() {
		long t = System.currentTimeMillis();
		while(true){
			// Read inputs
			double yBall = analogInPosition.get();
			double ref = referenceGenerator.getRef();
			
			synchronized (PIDController) { // To avoid parameter changes in between
				
				double u = PIDController.calculateOutput(yBall, ref);
				
				
				synchronized (PIController) {
					double yAngle = analogInAngle.get();
					// Compute control signal
					double uInner = limit(PIController.calculateOutput(yAngle, u), uMin, uMax);
					// Set output
					analogOut.set(uInner);
					PIController.updateState(uInner);
				}
				// Update state of outer controller
				PIDController.updateState(u);
				
			}
			
			analogRef.set(ref); // Only for the plotter animation
			
			t = t + PIDController.getHMillis();
			long duration = t - System.currentTimeMillis();
			if (duration > 0) {
				try {
					sleep(duration);
				} catch (InterruptedException e) {
				}
			}
			
		}

	}

	private double limit(double u, double umin, double umax) {
		if (u < umin) {
			u = umin;
		} else if (u > umax) {
			u = umax;
		}
		return u;
	}
}